clc,clear,close all
load("turn_sign.mat")
load("map_tokyo.mat")
%% GR-X
% GR=zeros(1,22100);
% for i=1:44200;
% GR(1,i) = GR_x(i);
% end

figure()
plot(GR)
title('GR-x')
%% 参数
CP = 400;
% fatigue = 0; %疲劳因子
fatigue = zeros(1,length(GR)+1);
tensity = 1; %GR影响规划功率的系数因子
P_real = ones(1,length(GR));
P_plan = ones(1,length(GR));
V = ones(1,length(GR));
v=0;%急弯上的切向速度
u=zeros(1,length(GR));%急弯上的向心速度
decrease =0.925;%入急弯速度衰减
r=4;%急弯半径
delta_T = zeros(1,length(GR));
%% fatigue无随机项
% fatigue_sum =zeros(1,length(GR)+1);
for i=1:1:length(GR)
    P_plan(1,i) = CP.*(1+tensity.*GR(1,i));
    
    
    P_real(1,i) = P_plan(1,i)*(1-fatigue(1,i));

    temp=Solve3Polynomial(0.1596,6*0.1596,(2.8243*cos(atan(GR(1,i)))+85*sin(atan(GR(1,i)))),9*0.1596-P_real(1,i));
    
    if(turn_sign(i)==0)
       V(1,i)=temp(imag(temp)==0);
    end

    if(turn_sign(i)==1)
       v =temp(imag(temp)==0).*decrease;
       u(1,i)=4.*(100.*pi./12./85./abs((v-V(1,i-1))./delta_T(1,i-1))-1);
       V(1,i)=sqrt(v.^2 + u(1,i).^2);
    end

    fatigue(1,i+1) = fatigue(1,i) + 0.00001.*atan(P_real(1,i) - CP);
%     fatigue_sum(1,1+i) =  P_real(1,i) - CP + fatigue_sum(1,i);
    if fatigue(1,i+1) <= 0
       fatigue(1,i+1) = 0;
    end
    
    if(i~=length(GR))
    delta_T(1,i)=1./((V(1,i)+V(1,i+1))./2);
    end
    if(i==length(GR))
    delta_T(1,i)=1./V(1,length(GR));
    end

end
%% 通过V-x图计算t

t(1,1) = 1./((V(1,1)+V(1,2))./2);
for i=1:1:length(GR)-1
    t(1,i+1)=t(1,i)+1./((V(1,i)+V(1,i+1))./2);
end
t(1,length(GR))=1./V(1,length(GR))+t(1,length(GR)-1);
T0 = t(1,length(GR))./60

for i=1:1:length(GR)-1
    delta_T(1,i)=1./((V(1,i)+V(1,i+1))./2);
end
%% 画图
figure()
plot(V)
title('V-x')
% 
% figure()
% plot(P_real)
% title('P real-x')
% 
% figure()
% plot(t,P_real)
% hold on
% CP=CP.*ones(1,length(GR));
% plot(t,CP)
% title('P real-t')
% 
% figure()
% plot(100000.*tan(fatigue))
% title('fatigue-x')

%% 判断做功是否超过W'
% W_real = zeros(1,length(GR));
% W_sum = zeros(1,length(GR));
% for i=1:1:length(GR)
%     W_real(1,i) = (P_real(1,i) - CP(1,i)).* delta_T(1,i);
% end
% 
% W_sum(1,1) = W_real(1,1);
% for i=1:1:length(GR)-1
%     W_sum(1,i+1) = W_sum(1,i) + W_real(1,i+1);
% end




